//
// Created by bit on 2022/8/3.
//

#ifndef UNTITLED3_BLOSFILTER_H
#define UNTITLED3_BLOSFILTER_H

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <fstream>
//#include "utils.h"
#include "sstream"

using namespace Eigen;

// radian
struct LOS_STATE {
    float vertical;
    float horizontal;
    float rate_ver;
    float rate_hor;
};

class BlosFilter {
public:
    BlosFilter() {
        std::string output_log_path = "blos_output_log.csv";
        output_data_.open(output_log_path.c_str());

        output_data_ << "wx,wy,wz,target_x,target_y,target_z,q1,q2,q3,q4,ture_BLOSV,ture_BLOSH,"
        << "x,y,z,image_timestamp,"
        << "BLOS_v,BLOS_h,LOS_v,LOS_h,filter_LOS_v,filter_LOS_h,LOS_vrate,LOS_hrate,dt_"
        << ",px4_timestamp,ture_LOS_v,ture_LOS_h,ture_LOS_vrate,ture_LOS_hrate,height" << std::endl;
    };
    ~BlosFilter() = default;

    bool update(float vangle, float hangle, LOS_STATE &state);

    void setInitState(float ver, float hor, float rate_ver, float rate_hor);
    void dataToLog(ObjectLOS BLOS, float vangle_before_filter, float hangle_before_filter,
                   LOS_STATE LOS_after_filter, const UavStatus &uav,
                   LOS_STATE true_LOS_after_filter) {
        output_data_ << uav.localPos.x << "," << uav.localPos.y << "," << uav.localPos.z
        << "," << BLOS.timestamp << "," << BLOS.vangle << "," << BLOS.hangle
        << "," << vangle_before_filter << "," << hangle_before_filter
        << "," << LOS_after_filter.vertical << "," << LOS_after_filter.horizontal
        << "," << LOS_after_filter.rate_ver << "," << LOS_after_filter.rate_hor
        << "," << dt_ << "," << uav.att.timestamp
        << "," << true_LOS_after_filter.vertical << "," << true_LOS_after_filter.horizontal
        << "," << true_LOS_after_filter.rate_ver << "," << true_LOS_after_filter.rate_hor
        << "," << -uav.localPos.z << std::endl;
    }

    /**
     * 将体视线角信息转换成绝对的视线角信息
     * @param BLOS 体坐标系下的视线角信息，及机体四元数
     * @param LOS_vertical
     * @param LOS_horizon
     */
    void BLOS2LOS(ObjectLOS BLOS, float &LOS_vertical, float &LOS_horizon,
                  const px4_msgs::msg::VehicleLocalPosition &uav_pos);

    void BLOS_sim(const px4_msgs::msg::VehicleLocalPosition &vehicle, const px4_msgs::msg::VehicleAttitude &att,
                  float N_target, float E_target, float D_target, float &vangle, float &hangle);

private:
    /**
     *
     * @param blos
     * @param x1_previous
     * @param x2_previoius
     * @param x1
     * @param x2
     */
    float diff_func(float x2, float dt, float blos, float x1_previous, float x2_previoius);

    int getSign(float input);

    float dichotomy(float min, float max, float dt, float blos, float x1_previous, float x2_previoius);

private:
    float v_init_, h_init_;

    uint64_t time_loop_{};

    std::ofstream blos_data_;
    uint64_t log_index{0};

    std::ofstream output_data_;

    uint8_t init_phase_{0};

    float R_{31.5f};
    float a0_{0.3f};
    float a1_{0.2f};
    float b0_{0.5f};
    float b1_{0.2f};
    float alpha_{0.8f};

    LOS_STATE los_previous_{};
    float dt_{0.1};

};

#endif //UNTITLED3_BLOSFILTER_H
